#ifndef __map_occupancy_grid_h__
#define __map_occupancy_grid_h__

#include <stdint.h>
#include <fstream>

typedef struct {
	float x;
	float y;
} vec2;

typedef struct {
	int lower_x;
	int lower_y;
	int upper_x;
	int upper_y;
} DirtyRect;

class OccupancyGrid
{
public:
	OccupancyGrid(int width, int height, float resolution);
	~OccupancyGrid();

	void writeToFile(std::ofstream& fileStream);
	void readFromFile(std::ifstream& fileStream);

	void add(vec2 from, vec2 to, bool hit);
	void addSensorRay(vec2 origin, float direction, float mean, float std_dev, float height);
	float probabilityOfSensorRay(vec2 origin, float direction, float mean, float std_dev, float height);
	void addHitWithDistribution(vec2 location, float std_dev, float height);
	void subtractRay(vec2 from, vec2 to, float scale);
	float probabilityOccupied(int cell_x, int cell_y);
	void update(int cell_x, int cell_y, int8_t value);

	float getResolution() { return resolution_; }
	int getHeight() { return height_; }
	int getWidth() { return width_; }

	void clearDirtyRect();
	DirtyRect getDirtyRect() { return dirty_rect_; }
private:
	float logoddsToProbabilitySlow(int8_t logodds);
	inline float logoddsToProbability(int8_t logodds);
	inline int8_t probabilityToLogodds(float probability);

	float logoddsLookup_[256];

	int8_t **grid_; // Contains the cells
	float resolution_; // Cells per meter
	int width_; // Number of cells in x-direction
	int height_; // Number of cells in y-direction

	DirtyRect dirty_rect_;
};

#endif
